from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import Twist

import rclpy
from rclpy.node import Node

from turtlesim.msg import Pose
from turtlesim.srv import Spawn


class PointPublisher(Node):

    def __init__(self):
        super().__init__("turtle_tf2_message_broadcaster")

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, "spawn")
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False
        self.result = None
        # if the topics of turtle3 can be subscribed
        self.turtle_pose_cansubscribe = False

        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        if not self.turtle_spawning_service_ready:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = "turtle3"
                request.x = 4.0
                request.y = 2.0
                request.theta = 0.0
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info("Service is not ready")
        elif self.result:
            if self.result.done():
                self.turtle_pose_cansubscribe = True
                self.result = None
            

        if self.turtle_pose_cansubscribe:
            self.vel_pub = self.create_publisher(Twist, "turtle3/cmd_vel", 10)
            self.sub = self.create_subscription(Pose, "turtle3/pose", self.handle_turtle_pose, 10)
            self.pub = self.create_publisher(PointStamped, "turtle3/turtle_point_stamped", 10)
            # 只创建一次 订阅,发布
            self.turtle_pose_cansubscribe = False

    def handle_turtle_pose(self, msg):
        vel_msg = Twist()
        vel_msg.linear.x = 1.0
        vel_msg.angular.z = 1.0
        self.vel_pub.publish(vel_msg)

        ps = PointStamped()
        ps.header.stamp = self.get_clock().now().to_msg()
        ps.header.frame_id = "world"
        ps.point.x = msg.x
        ps.point.y = msg.y
        ps.point.z = 0.0
        self.pub.publish(ps)


def main():
    rclpy.init()
    node = PointPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()


if __name__ == "__main__":
    main()
